/*!
  @file feature_matcher.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _FEATURE_MATCHER_H_
#define _FEATURE_MATCHER_H_

#include <vector>

#include <boost/scoped_ptr.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>

#include <ros/ros.h>

#include "mrover_vo/feature.h"
#include "mrover_vo/validity_checker.h"

namespace vo {

/*! 
 *  @brief  feature matcher class
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  detailed documentaiton for this class
 */

class FeatureMatcher {
  typedef typename ValidityChecker<FeatureMatcher, cv::Point2f>::ConstPtr ValidityCheckerConstPtr;
  typedef typename Feature::Ptr FeaturePtr;
  typedef typename Feature::ConstPtr FeatureConstPtr;
  typedef boost::shared_ptr<Feature::KeyPoints const> KeyPointsConstPtr; 
  typedef boost::shared_ptr<Feature::Descriptors const> DescriptorsConstPtr;
  typedef boost::shared_ptr<Feature::Matches> MatchesPtr;

 public:
  typedef boost::scoped_ptr<FeatureMatcher> Ptr;


  FeatureMatcher() 
      : matcher_(cv::DescriptorMatcher::create("BruteForce-Hamming"))
  { }

  ~FeatureMatcher() { }


  void setInputFeature(const KeyPointsConstPtr &kp, const DescriptorsConstPtr &desc) {
    input_kp_ = kp;
    input_desc_ = desc;
  }

  void setTargetFeature(const KeyPointsConstPtr &kp, const DescriptorsConstPtr &desc) {
    target_kp_ = kp;
    target_desc_ = desc;
  }

  void setOutputMatches(MatchesPtr &matches) { matches_ = matches; }
  inline MatchesPtr getMatches() const { return matches_; }

  void addValidityCheck(const ValidityCheckerConstPtr &checker) { validity_checker_.push_back(checker); }
  void deleteValidityCheck() { validity_checker_.clear(); }

  void compute();
  inline void compute(MatchesPtr &match) {
    setOutputMatches(match);
    compute();
  }

 private:
  bool initCompute() {
    if (!input_desc_ || !target_desc_ || !matches_) return false;
    return true;
  }

  void radiusMatch(const float MAX_RADIUS=100);
  void filterWeakMatches();
  void checkMutualConsistency();
  void checkForValidity();

  cv::Ptr<cv::DescriptorMatcher> matcher_;
  std::vector<ValidityCheckerConstPtr> validity_checker_;

  KeyPointsConstPtr input_kp_;
  KeyPointsConstPtr target_kp_;
  DescriptorsConstPtr input_desc_;
  DescriptorsConstPtr target_desc_;
  MatchesPtr matches_;

  friend class ValidityChecker<FeatureMatcher, cv::Point2f>;
};

}  // namespace vo

#endif
